// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MotionPlanCHOMP.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Collision;
    using Microsoft.Robotics.Geometry;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Tracing;

    using ShoNS.Array;

    /// <summary>
    /// Functional Gradient Based Optimization Planner - CHOMP 
    /// </summary>
    //// http://www.ri.cmu.edu/publication_view.html?pub_id=7421
    //// \\robofile\Baker\Shared\Users\arun\joint_limit_constraint_wholetraj.pdf 
    public class MotionPlanCHOMP : IDisposable
    {
        /// <summary>
        /// Trace context for this class
        /// </summary>
        private static TraceContext chompContext = TraceContexts.FromString(TraceContexts.Manip, "CHOMP");

        /// <summary>
        /// Instance of the CHOMP parameters class that holds all the parameters set by the user
        /// </summary>
        private CHOMPparameters param;

        /// <summary>
        /// Robot Kinematic chain passed in to CHOMP
        /// </summary>
        private KinematicChain robotKinematicChain;

        /// <summary>
        /// Number of internal points (excluding start and goal points)
        /// </summary>
        private int numInternal;

        /// <summary>
        /// Number of Degrees of Freedom for the robot 
        /// </summary>
        private int numDOF;

        /// <summary>
        /// Time difference between two adjacent trajectory points
        /// </summary>
        private double dt;

        /// <summary>
        /// Internal points of the trajectory (Excluding start and end points)
        /// </summary>
        private DoubleArray jointTrajectoryInternal;

        /// <summary>
        /// Finite difference matrix that returns velocity when applied to the trajectory
        /// velocity = K * trajectory + E (E is for end points)
        /// </summary>
        private DoubleArray kmat;

        /// <summary>
        /// Matrix to take care of end points. Part of velocity computation.
        /// </summary>
        private DoubleArray emat;

        /// <summary>
        /// Smoothness Metric (A = scalar * K * K^T)
        /// </summary>
        private DoubleArray smoothMetric;

        /// <summary>
        /// Inverse of the Smoothness Metric (A-inverse)
        /// </summary>
        private DoubleArray smoothMetricInverse;

        /// <summary>
        /// Linear term in the smoothness cost (B = scalar * K * E^T) (Cost =  x^T A x + x^T B + C)
        /// </summary>
        private DoubleArray smoothLinearTerm;

        /// <summary>
        /// Constant term in the smoothness cost (C = scalar * trace (E * E^T)) (Cost =  x^T A x + x^T B + C)
        /// </summary>
        private double smoothConstTerm;

        /// <summary>
        /// Gradient for each trajectory point (Smoothness + Obstacle gradient) - exclude fixed end points which have 0 gradient
        /// </summary>
        private DoubleArray jointGradientInternal;

        /// <summary>
        /// Obstacle gradient for each trajectory point - exclude fixed end points which have 0 gradient
        /// </summary>
        private DoubleArray obsGradient;

        /// <summary>
        /// Obstacle cost for each internal trajectory point - exclude fixed end points which have 0 cost
        /// </summary>
        private DoubleArray obsCost;

        /// <summary>
        /// Sphere elements that make up the robot collision model
        /// </summary>
        private List<SphereElement> robotSpheres;

        /// <summary>
        /// Index of the link on which the corresponding robot sphere lies in
        /// </summary>
        private List<int> linkIndexOfRobotSphere; 

        /// <summary>
        /// 3D positions of each of the robot spheres for each trajectory point
        /// </summary>
        private double[][][] spherePositions;

        /// <summary>
        /// 3D velocities of each of the spheres for each trajectory point
        /// </summary>
        private double[][][] sphereVelocities;

        /// <summary>
        /// 3D accelerations of each of the spheres
        /// </summary>
        private double[][][] sphereAccelerations;

        /// <summary>
        /// Jacobian for each of the spheres for each trajectory point
        /// </summary>
        private DoubleArray[][] sphereJacobians;

        /// <summary>
        /// Delegate function that we call to compute the obstacle cost and gradient
        /// </summary>
        private Compute3DCostandGradientDelegate obsDelegate;

        /// <summary>
        /// Minimum allowable vector norm for computational stability (used to bound the norms as we divide by them when computing unit vectors)  
        /// </summary>
        private double minVectorNorm = 0.000001; // 1e-6 

        // TODO:
        // 1) Check if we can avoid using weights in smoothness matrix generation - Ainv - weighting later should be enough
        // 4) See if we can use BFGS with the algorithm. Does that help a lot?

        /// <summary>
        /// Initializes a new instance of the <see cref="MotionPlanCHOMP" /> class
        /// </summary>
        /// <param name="chain">Kinematic chain </param>
        /// <param name="initParams"> Set of parameters for initializing CHOMP. See the class "CHOMPPARAMETERS.CS" for details </param>
        /// <param name="obsDelegate"> Delegate function (function handle) for a function to compute the workspace cost and gradient for obstacles </param>
        public MotionPlanCHOMP(
            KinematicChain chain, 
            CHOMPparameters initParams, 
            Compute3DCostandGradientDelegate obsDelegate)
        {
            // Create a new stopwatch for timing and start it
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Start();

            // Set the kinematic chain and get the number of spheres
            this.robotKinematicChain = chain;
            this.GetSpheresInKinematicChain(this.robotKinematicChain);

            // Get num of DOF for each trajectory point
            this.numDOF = this.robotKinematicChain.JointList.Length; // number of DOF

            // Setup the various parameters from the user fed values
            this.param = initParams;
            if (this.param.NumPoints < 3)
            {
                throw new ArgumentException(" Trajectory needs to have atleast 3 points. Numpoints : "
                    + this.param.NumPoints.ToString() + " < 3");
            }

            // Get joint limits
            this.LowLimits = new DoubleArray(this.numDOF);
            this.HighLimits = new DoubleArray(this.numDOF);
            for (int i = 0; i < this.numDOF; i++)
            {
                this.LowLimits[i] = this.robotKinematicChain.JointList[i].MinValue;
                this.HighLimits[i] = this.robotKinematicChain.JointList[i].MaxValue;
            }

            // Initialize the joint trajectory to all zeros (NDOF X NUMPOINTS)
            this.JointTrajectory = new DoubleArray(this.numDOF, this.param.NumPoints);

            // Setup number of internal trajectory points
            this.numInternal = this.param.NumPoints - 2; // By default, the end points are fixed
            this.dt = 1.0 / (this.param.NumPoints - 1); // dt = 1/(n - 1) - constant deltaT between points

            // Setup variables for the trajectory, gradients and obstacle costs for all the internal points
            this.obsGradient = new DoubleArray(this.numDOF, this.numInternal);
            this.obsCost = new DoubleArray(this.numInternal, 1);
            this.jointTrajectoryInternal = new DoubleArray(this.numDOF, this.numInternal);
            this.jointGradientInternal = new DoubleArray(this.numDOF, this.numInternal);

            // Setup collision flags (obstacle and self) for the internal points of the trajectory (excluding start and goal)
            this.IsInternalPointInCollisionWithObstacles = new bool[this.numInternal];
            this.IsInternalPointInCollisionWithSelf = new bool[this.numInternal];

            // Setup the arrays for storing sphere positions, velocities, accelerations and jacobians
            // We compute positions, velocities, accelerations and jacobians for each trajectory point for each sphere
            this.spherePositions = new double[this.param.NumPoints][][];
            this.sphereVelocities = new double[this.param.NumPoints][][];
            this.sphereAccelerations = new double[this.param.NumPoints][][];
            this.sphereJacobians = new DoubleArray[this.param.NumPoints][]; 

            // At each trajectory point, we have to compute position, velocity, acceleration & jacobian of 
            // numSpheres spheres.
            for (int i = 0; i < this.param.NumPoints; i++)
            {
                this.spherePositions[i] = new double[this.robotSpheres.Count][];
                this.sphereVelocities[i] = new double[this.robotSpheres.Count][];
                this.sphereAccelerations[i] = new double[this.robotSpheres.Count][];
                this.sphereJacobians[i] = new DoubleArray[this.robotSpheres.Count]; 
            }

            // Now do inner vector3 and doublearray (it is a 6 x n matrix where n is numDOF) additions
            for (int i = 0; i < this.param.NumPoints; i++)
            {
                for (int j = 0; j < this.robotSpheres.Count; j++)
                {
                    this.spherePositions[i][j] = new double[3]; // [X;Y;Z]
                    this.sphereVelocities[i][j] = new double[3]; // [X;Y;Z]
                    this.sphereAccelerations[i][j] = new double[3]; // [X;Y;Z]
                    this.sphereJacobians[i][j] = new DoubleArray(3, this.numDOF);
                }
            }

            // Initialize the smoothness metric and its inverse
            this.CreateSmoothnessMetricAndInverse();

            // Setup the delegate function for computing the obstacle cost
            // Set it to the passed in function
            this.obsDelegate = new Compute3DCostandGradientDelegate(obsDelegate);

            // Display total time
            TraceOut.Info(chompContext, " \t CHOMP Initialization complete. Total time taken: {0} sec ", ((double)stopwatch.ElapsedMilliseconds) * 1e-3);  
        }

        /// <summary>
        /// Delegate of the function that computes the cost for a sphere and gradient at the sphere center in the environment
        /// Has to be implemented by the user of the motion planner. Examples are at the end of MotionPlanCHOMP.cs
        /// </summary>
        /// <param name="pos"> 3D position of the center of the sphere </param>
        /// <param name="radius"> Radius of the sphere </param>
        /// <param name="epsilon"> Threshold for collision with the environment. Incurs cost if robot is within epsilon distance from the obstacle surface. </param>
        /// <param name="cost"> Cost of that sphere - Output (NOTE: This needs to be POSITIVE if the sphere is within "EPSILON" of the obstacle) </param>
        /// <param name="grad"> Gradient at the sphere center (3D) - Output </param>
        /// <returns> Flag indicating whether the sphere is in collision </returns>
        public delegate bool Compute3DCostandGradientDelegate(double[] pos, double radius, double epsilon, out double cost, out double[] grad);

        /// <summary>
        /// Gets the joint trajectory
        /// </summary>
        public DoubleArray JointTrajectory { get; private set; }

        /// <summary>
        /// Gets the Low limits for the robot
        /// </summary>
        public DoubleArray LowLimits { get; private set; }

        /// <summary>
        /// Gets the High limits for the robot
        /// </summary>
        public DoubleArray HighLimits { get; private set; }

        /// <summary>
        /// Gets the type of convergence achieved (OUTPUT OF CHOMP)
        ///     1 - Reached maximum number of iterations
        ///     2 - Cost change in successive iterations was less than threshold
        ///     3 - Reached maximum time budget
        ///     4 - Trajectory outside of collision (only if "ExitOnNoCollision" is set to true)
        /// </summary>
        public int ConvergenceType { get; private set; }

        /// <summary>
        /// Gets the Smoothness cost for the whole trajectory
        /// </summary>
        public double SmoothnessCostSum { get; private set; }

        /// <summary>
        /// Gets the Obstacle cost for the whole trajectory (including self collision cost)
        /// </summary>
        public double ObstacleCostSum { get; private set; }

        /// <summary>
        /// Gets the obstacle collision flags for all the internal points of the trajectory (excluding start and goal)
        /// </summary>
        public bool[] IsInternalPointInCollisionWithObstacles { get; private set; }

        /// <summary>
        /// Gets the self-collision flags for all the internal points of the trajectory
        /// </summary>
        public bool[] IsInternalPointInCollisionWithSelf { get; private set; }

        /// <summary>
        /// Gets a value indicating whether the internal part of the trajectory is in collision with obstacles (excluding start and goal)
        /// </summary>
        public bool IsInternalTrajectoryInCollisionWithObstacles { get; private set; }

        /// <summary>
        /// Gets a value indicating whether the internal part of the trajectory is in self collision (excluding start and goal)
        /// </summary>
        public bool IsInternalTrajectoryInCollisionWithSelf { get; private set; }

        /// <summary>
        /// Generate a trajectory using linear interpolation between the start and end points
        /// </summary>
        /// <param name="start"> Start joint configuration </param>
        /// <param name="end"> End joint configuration </param>
        /// <param name="numTrajPoints"> Number of points on the trajectory (including start and end) </param>
        /// <returns>A trajectory</returns>
        public static DoubleArray GenerateTrajectoryUsingLinearInterpolation(DoubleArray start, DoubleArray end, int numTrajPoints)
        {
            DoubleArray traj = new DoubleArray(start.size0, numTrajPoints);

            for (int i = 0; i < numTrajPoints; ++i)
            {
                DoubleArray intermediateConfig = start + (end - start) * (double)i / (double)(numTrajPoints - 1);

                for (int j = 0; j < traj.size0; ++j)
                {
                    traj[j, i] = intermediateConfig[j, 0];
                }
            }

            return traj;
        }

        /// <summary>
        /// Get the list of points in a double array - each column is considered a separate point
        /// </summary>
        /// <param name="trajectory"> Input trajectory </param>
        /// <param name="isEachColumnAWaypoint"> Boolean that indicates if the waypoints are columns or rows</param>
        /// <returns> List of waypoints in the trajectory </returns>
        public static List<double[]> GetWayPointList(DoubleArray trajectory, bool isEachColumnAWaypoint = true)
        {
            // Check if we go along rows or columns
            DoubleArray traj = trajectory;
            if (!isEachColumnAWaypoint)
            {
                 traj = traj.T;
            }

            // Assume that we always have points along columns (each column is a point)
            int numPts = traj.size1; // number of points
            int dimPerPt = traj.size0; // dimension of each point

            // Store the trajectory in a list for display
            List<double[]> wayPoints = new List<double[]>();
            for (int i = 0; i < numPts; i++)
            {
                double[] p = new double[dimPerPt];
                for (int j = 0; j < dimPerPt; j++)
                {
                    p[j] = traj[j, i];
                }

                wayPoints.Add(p);
            }

            return wayPoints;
        }

        /// <summary>
        /// Smoothness cost is defined as: C_s = 0.5 * || K * x + e ||^2 = 0.5 * x A x^T + x B + C
        /// A = K K^T, B = K e, C = 0.5 * e e^T
        /// Here, K is a finite difference matrix. For now, it is just the first derivative finite difference matrix
        /// e is a vector that takes care of end points
        /// In this function, we compute the quadratic term "A" and its inverse which are independent of the start and goal points and depend only on the number of trajectory points
        /// </summary>
        public void CreateSmoothnessMetricAndInverse()
        {
            // Number of velocity values we can compute for the trajectory
            int numDerivs = this.param.NumPoints - 1;  // also equal to numInternal + 1

            // Create the finite difference matrix "K" for velocity. For "N" points, we have N-1 velocities.
            // Here, N = numPoints, N-1 = numPoints-1 = numInternal+1
            // K takes care of internal points. So, we have size of K = numInternal x numDerivs
            // End points are fixed, so do not consider them. We have "emat" to take care of them
            this.kmat = new DoubleArray(this.numInternal, numDerivs); // initialized to 0

            // Each column of K has -1 & 1 in it such that, for column "i", we get x*K = (x(j+1) - x(j))/dt
            // except for the first and last row which need "e" added to it to get the result
            double oneByDt = 1.0 / this.dt;
            for (int i = 0; i < this.kmat.size1; i++)
            {
                // First internal point
                if (i == 0) 
                {
                    this.kmat[i, i] = oneByDt;
                }
                else if (i == (numDerivs - 1)) 
                {
                    // Last internal point 
                    this.kmat[i - 1, i] = -oneByDt; 
                }
                else
                {
                    // First derivative -> (x(j+1) - x(j))/dt
                    this.kmat[i, i] = oneByDt;
                    this.kmat[i - 1, i] = -oneByDt;
                }
            }

            // Compute the matrices A and Ainverse
            // We scale each matrix by the corresponding smoothness weight and 
            // compute the average cost by dividing by number of velocity values we compute 
            // A = K * K^T
            this.smoothMetric = (this.param.SmoothWeight / numDerivs) * this.kmat * this.kmat.T; // this is of size "NUMINTERNAL x NUMINTERNAL" and is same for all DOFs

            // Compute the inverse of the smoothness matrix - A^-1. We use this when we do gradient update
            this.smoothMetricInverse = this.smoothMetric.Inv();
        }

        /// <summary>
        /// Smoothness cost is defined as: C_s = 0.5 * || K * x + e ||^2 = 0.5 * x A x^T + x B + C
        /// A = K K^T, B = K e, C = 0.5 * e e^T
        /// Here, K is a finite difference matrix. For now, it is just the first derivative finite difference matrix
        /// e is a vector that takes care of end points
        /// In this function, we compute the linear and constant terms B and C which depend on the start and goal points of the trajectory 
        /// </summary>
        /// <param name="initTrajectory"> Initial trajectory for CHOMP to optimize. </param>
        public void CreateSmoothnessLinearAndConstantTerms(DoubleArray initTrajectory)
        {
            // Number of velocity values we can compute for the trajectory
            int numDerivs = this.param.NumPoints - 1;  // also equal to numInternal + 1
            double oneByDt = 1.0 / this.dt;

            // Initialize the adjustment for the start and end points
            this.emat = new DoubleArray(this.numDOF, numDerivs); // initialized to 0

            // Set first row equal to -START
            // Set last row equal to END - last trajectory point (goal)
            for (int j = 0; j < this.emat.size0; j++)
            {
                this.emat[j, 0] = -initTrajectory[j, 0] * oneByDt;
                this.emat[j, numDerivs - 1] = initTrajectory[j, this.param.NumPoints - 1] * oneByDt;
            }

            // Compute the linear and constant terms B & C
            // B = K * e^T
            this.smoothLinearTerm = (this.param.SmoothWeight / numDerivs) * this.kmat * this.emat.T; // this is of size "NUMDOF x NUMINTERNAL"; one vector per DOF

            // C = 0.5 * trace(e * e^T) -> scalar which is the sum over all joints (e * e^T for each joint)
            this.smoothConstTerm = 0.5 * (this.param.SmoothWeight / numDerivs) * (this.emat * this.emat.T).Trace(); // trace of the matrix
        }

        /// <summary>
        /// Optimize the initial trajectory to generate a smooth collision-free trajectory
        /// NOTE: CHOMP cannot generate a guaranteed collision free trajectory as it is minimizing a combination of smoothness and obstacle cost which tend to fight each other
        /// </summary>
        /// <param name="initTrajectory"> Initial trajectory for CHOMP. CHOMP optimizes this trajectory for smoothness and obstacle cost. </param>
        /// <returns> A joint trajectory after CHOMP optimization that locally minimizes smoothness and obstacle costs </returns>
        public DoubleArray RunCHOMPOptimization(DoubleArray initTrajectory)
        {
            // Create a new stopwatch for timing and start it
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Start();

            // Do adhoc check for ensuring that the constructor has been called
            if (null == this.JointTrajectory)
            {
                throw new ArgumentException(" Joint trajectory has not been initialized. Check if constructor was invoked. ");
            }

            // Make sure that the initial trajectory is of correct shape
            if ((initTrajectory.size0 != this.JointTrajectory.size0) || (initTrajectory.size1 != this.JointTrajectory.size1))
            {
                throw new ArgumentException(" Initial trajectory is not the correct shape! Is: "
                    + initTrajectory.Size.ToString() + ". Has to be: " + this.JointTrajectory.Size.ToString());
            }

            // Initialize the joint trajectory based on the initial trajectory input
            for (int j = 0; j < initTrajectory.size0; j++)
            {
                for (int k = 0; k < initTrajectory.size1; k++)
                {
                    this.JointTrajectory[j, k] = initTrajectory[j, k];
                }
            }

            // Pre-compute some terms for the smoothness cost computation (smoothlinearterm and smoothconstantterm which both depend on the start and goal points) 
            this.CreateSmoothnessLinearAndConstantTerms(initTrajectory);

            // Display time for pre-computation
            TraceOut.Info(chompContext, " \t Time for initial pre-computation: {0} sec ", ((double)stopwatch.ElapsedMilliseconds) * 1e-3);  
            TraceOut.Info(chompContext, " \t Starting CHOMP iterations now.");

            // Initialize variables for the convergence options
            int i = 0; // Iteration counter
            double cost_prev = double.MaxValue; // variable to store previous cost

            // Run the CHOMP optimization until one of the convergence options are satisfied
            bool converged = false;
            while (!converged)
            {
                // Precompute position, velocity, accelerations & jacobians for each robot sphere
                this.DoPreComputationsForCHOMPIteration();

                // Run the CHOMP iteration - compute and use gradient at each iteration
                double smoothCostSum, obstacleCostSum;
                this.RunCHOMPIteration(true, out smoothCostSum, out obstacleCostSum);

                // Check for collision
                // Set trajectory collision flags to false initially
                this.IsInternalTrajectoryInCollisionWithObstacles = false;
                this.IsInternalTrajectoryInCollisionWithSelf = false;

                // Check obstacle collision flags for all the trajectory points. If the robot collides with an obstacle at even a single waypoint, the trajectory is in collision
                for (int j = 0; j < this.IsInternalPointInCollisionWithObstacles.Length; j++)
                {
                    if (this.IsInternalPointInCollisionWithObstacles[j])
                    {
                        this.IsInternalTrajectoryInCollisionWithObstacles = true;
                        break;
                    }
                }

                // Get overall collision flag
                bool isTrajectoryInCollisionWithEnvironment = this.IsInternalTrajectoryInCollisionWithObstacles; // if we are not asked to use self collision flag, don't use it
                if (this.param.UseSelfCollisionInConvergenceTesting)
                {
                    // Check self collision flags for all the trajectory points. If the robot collides with self at even a single waypoint, the trajectory is in collision
                    for (int j = 0; j < this.IsInternalPointInCollisionWithSelf.Length; j++)
                    {
                        if (this.IsInternalPointInCollisionWithSelf[j])
                        {
                            this.IsInternalTrajectoryInCollisionWithSelf = true;
                            break;
                        }
                    }

                    // Use both self collision and external obstacle collision flags
                    isTrajectoryInCollisionWithEnvironment = (this.IsInternalTrajectoryInCollisionWithObstacles || this.IsInternalTrajectoryInCollisionWithSelf);
                }

                // Get cost data
                this.SmoothnessCostSum = smoothCostSum;
                this.ObstacleCostSum = obstacleCostSum;
                double cost = this.SmoothnessCostSum + this.ObstacleCostSum;

                // Display statistics of iteration
                TraceOut.Info(
                    chompContext,
                    "Iteration: {0}, Smoothness Cost: {1}, Obstacle cost: {2}, Total: {3} ", 
                    i,
                    this.SmoothnessCostSum,
                    this.ObstacleCostSum, 
                    cost);
                TraceOut.Info(
                    chompContext,
                    " \t Difference in cost from previous iteration : {0}, Time Elapsed: {1} sec ",
                    cost - cost_prev, 
                    ((double)stopwatch.ElapsedMilliseconds) * 1e-3);  

                // Check for convergence - whichever comes earlier
                double costDiff = Math.Abs(cost - cost_prev);
                double elapsedTime = ((double)stopwatch.ElapsedMilliseconds) * 1e-3;
                if ((this.param.ExitOnNoCollision) && (!isTrajectoryInCollisionWithEnvironment))
                {
                    TraceOut.Info(chompContext, " --------- [SUCCESS] Exiting as the PREVIOUS iteration's trajectory is not in collision and EXITONNOCOLLISION flag is set ---------- ");
                    System.Console.WriteLine(" CHOMP: [SUCCESS] Final trajectory is not in collision! ");
                    TraceOut.Info(chompContext, " --------- Rewinding last CHOMP update ---------- ");

                    this.RewindLastCHOMPUpdate(); // rewind last chomp update
                    TraceOut.Info(chompContext, " --------- DONE --------- ");

                    // Set convergence flag and type
                    converged = true;
                    this.ConvergenceType = 4;
                }
                else if (i >= this.param.MaxIterations - 1)
                {
                    TraceOut.Info(
                        chompContext,
                        " --------- [ITERATION LIMIT EXCEEDED] Maximum number of iterations ({0}) reached. ---------- ",
                        this.param.MaxIterations);

                    // Set convergence flag and type
                    converged = true;
                    this.ConvergenceType = 1;
                }
                else if (costDiff <= this.param.MinCostDifference)
                {
                    TraceOut.Info(
                        chompContext,
                        " --------- [CONVERGED] Abs Difference in cost between successive iterations ({0}) is less than threshold ({1}). ---------- ",
                        costDiff,
                        this.param.MinCostDifference);

                    // Set convergence flag and type
                    converged = true;
                    this.ConvergenceType = 2;
                }
                else if (elapsedTime >= this.param.MaxTime)
                {
                    TraceOut.Info(
                        chompContext,
                        " --------- [TIME LIMIT EXCEEDED] Total time elapsed ({0} sec) is greater than the maximum allowed time budget ({1} sec). ---------- ",
                        elapsedTime,
                        this.param.MaxTime);

                    // Set convergence flag and type
                    converged = true;
                    this.ConvergenceType = 3;
                }

                // Increment iteration counter and update cost
                i++;
                cost_prev = cost;

                // Run an additional CHOMP iteration to correct for joint limit violations 
                // as well as for computing costs for the final trajectory
                // Run one last CHOMP iteration setting the gradient to zero. 
                // This will try to fix any joint limit violations ( may not succeed in fixing all violations as fixing one may cause others to violate)
                // We may have to run this in a loop checking for joint limit violations - maybe fold this inside the function (if we set gradient to zero, run limit fix in a loop)
                if (converged)
                {
                    // Display obstacle collision details for user
                    if (this.IsInternalTrajectoryInCollisionWithObstacles)
                    {
                        TraceOut.Info(chompContext, "[FINAL] Final trajectory is in collision with obstacles! ");
                        System.Console.WriteLine(" CHOMP: [FAIL] Final trajectory is in collision with obstacles! ");
                    }

                    // Display self collision details for user
                    if (this.IsInternalTrajectoryInCollisionWithSelf && this.param.UseSelfCollisionInConvergenceTesting)
                    {
                        TraceOut.Info(chompContext, "[FINAL] Final trajectory is in self collision! ");
                        System.Console.WriteLine(" CHOMP: [FAIL] Final trajectory is in self collision! ");
                    }

                    // Precompute position, velocity, accelerations & jacobians for each robot sphere
                    this.DoPreComputationsForCHOMPIteration();

                    // Run the CHOMP iteration - set gradient to zero
                    double smoothCostSumFinal, obsCostSumFinal;
                    this.RunCHOMPIteration(false, out smoothCostSumFinal, out obsCostSumFinal);
                    TraceOut.Info(chompContext, "Running final CHOMP iteration to correct joint limit violations if present. ");
                    TraceOut.Info(
                        chompContext,
                        "[FINAL] Smoothness Cost: {0}, Obstacle cost: {1}, Total: {2} ",
                        smoothCostSumFinal, 
                        obsCostSumFinal, 
                        smoothCostSumFinal + obsCostSumFinal);
                    TraceOut.Info(
                        chompContext,
                        " \t Difference in cost from previous iteration : {0}, Total Time Elapsed: {1} sec ",
                        smoothCostSumFinal + obsCostSumFinal - cost_prev, 
                        ((double)stopwatch.ElapsedMilliseconds) * 1e-3);

                    // Final printout
                    System.Console.WriteLine(" CHOMP: Total Iterations: {0}, Time Elapsed: {1} ", i, ((double)stopwatch.ElapsedMilliseconds) * 1e-3);
                }
            }

            return this.JointTrajectory;
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="MotionPlanCHOMP"/> class.
        /// </summary>
        public void Dispose()
        {
            this.Dispose(true);
        }

        /// <summary>
        /// Disposes of an instance of the <see cref="MotionPlanCHOMP"/> class.
        /// </summary>
        /// <param name="disposing">True if disposing, false if finalizing.</param>
        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                if (null != this.jointTrajectoryInternal)
                {
                    this.jointTrajectoryInternal.Dispose();
                    this.jointTrajectoryInternal = null;
                }

                if (null != this.jointGradientInternal)
                {
                    this.jointGradientInternal.Dispose();
                    this.jointGradientInternal = null;
                }

                if (null != this.kmat)
                {
                    this.kmat.Dispose();
                    this.kmat = null;
                }

                if (null != this.emat)
                {
                    this.emat.Dispose();
                    this.emat = null;
                }

                if (null != this.sphereJacobians)
                {
                    for (int i = 0; i < this.sphereJacobians.Length; i++)
                    {
                        if (this.sphereJacobians[i] != null)
                        {
                            for (int j = 0; j < this.sphereJacobians[i].Length; j++)
                            {
                                this.sphereJacobians[i][j].Dispose();
                                this.sphereJacobians[i][j] = null;
                            }
                        }
                    }
                }

                if (null != this.smoothMetric)
                {
                    this.smoothMetric.Dispose();
                    this.smoothMetric = null;
                }

                if (null != this.smoothMetricInverse)
                {
                    this.smoothMetricInverse.Dispose();
                    this.smoothMetricInverse = null;
                }

                if (null != this.smoothLinearTerm)
                {
                    this.smoothLinearTerm.Dispose();
                    this.smoothLinearTerm = null;
                }

                if (null != this.obsGradient)
                {
                    this.obsGradient.Dispose();
                    this.obsGradient = null;
                }

                if (null != this.obsCost)
                {
                    this.obsCost.Dispose();
                    this.obsCost = null;
                }

                if (null != this.JointTrajectory)
                {
                    this.JointTrajectory.Dispose();
                    this.JointTrajectory = null;
                }

                if (null != this.HighLimits)
                {
                    this.HighLimits.Dispose();
                    this.HighLimits = null;
                }

                if (null != this.LowLimits)
                {
                    this.LowLimits.Dispose();
                    this.LowLimits = null;
                }
            }
        }

        /// <summary>
        /// Run a single iteration of CHOMP
        /// </summary>
        /// <param name="doUpdate"> Flag indicating if we want to take a gradient step. If we don't take a step, this just prints out the cost and checks for joint limit constraints </param>
        /// <param name="smoothCostSum"> Smoothness cost for each trajectory point (output) </param>
        /// <param name="obstacleCostSum"> Obstacle cost for each trajectory point (output) </param>
        private void RunCHOMPIteration(bool doUpdate, out double smoothCostSum, out double obstacleCostSum)
        {
            // Get the internal points (leave out first and last points)
            for (int i = 0; i < this.numDOF; i++)
            {
                for (int j = 0; j < this.numInternal; j++)
                {
                    this.jointTrajectoryInternal[i, j] = this.JointTrajectory[i, j + 1];
                }
            }

            // Compute the smoothness cost for the current trajectory
            // Smoothness cost is defined as: C_s = 0.5 * || K * x + e ||^2 = 0.5 * x A x^T + x B + C
            DoubleArray temp = this.jointTrajectoryInternal * this.smoothMetric * this.jointTrajectoryInternal.T; // Quadratic term -> 0.5 * x A x^T 
            smoothCostSum = 0.5 * temp.Trace() + (this.jointTrajectoryInternal * this.smoothLinearTerm).Trace() + this.smoothConstTerm; 
            
            // In case we are asked to take a gradient descent step
            // CHOMP gradient descent update is:
            // x_(t+1) = x_t - stepSize * gradient * A^-1 
            // Gradient is weighted sum of smoothness and obstacle gradients
            // gradient = smoothWt * smoothGrad + obsWt * obsGrad

            // Smoothness Gradient      
            // Gradient of smoothness cost = x A + B - Consider only internal points for "X"
            DoubleArray smoothGradient = this.jointTrajectoryInternal * this.smoothMetric + this.smoothLinearTerm.T;
           
            // Obstacle cost and gradient computation. We also get flags for collision with obstacles and self collision
            // Obstacle cost is Line integral of the cost of each robot point as it moves through a scalar potential in workspace
            this.ComputeObsCostAndGradient();

            // Get the average obstacle cost and gradient (these variables are set in the function ComputeObsCostAndGradient())
            obstacleCostSum = this.obsCost.Sum() / this.numInternal;
            this.obsGradient /= this.numInternal;

            // Overall joint gradient - in case we are not doing an update, we set gradient to zero (we still do joint limit check in this case)
            if (doUpdate)
            {
                // NOTE: Scaling by smoothWeight has already been taken into account through "A" and "B"
                for (int i = 0; i < this.numDOF; i++)
                {
                    for (int j = 0; j < this.numInternal; j++)
                    {
                        this.jointGradientInternal[i, j] = smoothGradient[i, j] + this.param.ObsWeight * this.obsGradient[i, j];
                    }
                }
            }
            else
            {
                // Set gradient to zero
                this.jointGradientInternal.FillValue(0.0);
            }

            // Refer to the paper: Space-Time Functional Gradient Optimization for Motion Planning for more details on the joint limit derivation
            // Also look at the file \\robofile\Baker\Shared\Users\arun\joint_limit_constraint_wholetraj.pdf for the actual derivation of the constrained update rule implemented here
            // We compute the violation of the limits at each trajectory point (if any) and cancel out the violation 
            // smoothly. This cancels out the violation exactly at the current point, but it may lead other points to violation
            // So, we may need to do this in an iterative manner.
            // We compute the KKT factors for each point that violates the limits. This is done with a matrix inversion step:
            //      LHS = W^-1 * RHS
            // where LHS are the KKT factors, RHS are the violations + gradient for those points & W is a sub-matrix of the inverse of the smoothness metric

            // Compute the Ainv spread gradient
            DoubleArray inverseSpreadGradient = this.jointGradientInternal * this.smoothMetricInverse;
            double lambda = 1.0 / this.param.StepSize;

            // The joint limit updates for each joint are independent of the rest, so we compute them separately
            Parallel.For(
                0,
                this.numDOF,
                (i) =>
                {
                    // Find out the points that violate the Limits and compute the RHS at these points
                    List<int> violIndices = new List<int>();
                    for (int j = 0; j < this.numInternal; j++)
                    {
                        if ((this.jointTrajectoryInternal[i, j] < this.LowLimits[i]) || (this.jointTrajectoryInternal[i, j] > this.HighLimits[i]))
                        {
                            violIndices.Add(j);
                        }
                    }

                    // In case we have points violating the limits, we try to correct the violations
                    if (violIndices.Count > 0)
                    {
                        // Display number of joint limit violations
                        TraceOut.Info(chompContext, " Joint: {0}, Num violations: {1}", i, violIndices.Count);

                        // Compute the RHS and the matrix W
                        DoubleArray rhs = new DoubleArray(violIndices.Count, 1);
                        DoubleArray w = new DoubleArray(violIndices.Count, violIndices.Count);
                        for (int x = 0; x < violIndices.Count; x++)
                        {
                            // Get the index of the point violating the limit
                            int j = violIndices[x];

                            // Violating low limits: RHS = -(A^-1 * configSpaceGrad)_x - (1/step_size) * violation
                            // Violating high limits: RHS = -(A^-1 * configSpaceGrad)_x + (1/step_size) * violation
                            if (this.jointTrajectoryInternal[i, j] < this.LowLimits[i])
                            {
                                rhs[x] = -inverseSpreadGradient[i, j] - lambda * (this.LowLimits[i] - this.jointTrajectoryInternal[i, j]);
                            }
                            else if (this.jointTrajectoryInternal[i, j] > this.HighLimits[i])
                            {
                                rhs[x] = -inverseSpreadGradient[i, j] + lambda * (this.jointTrajectoryInternal[i, j] - this.HighLimits[i]);
                            }

                            // Compute elements of matrix W. W is a submatrix of A^-1 with the elements from the indices violating the limits
                            for (int y = 0; y < violIndices.Count; y++)
                            {
                                // Get the index of the point violating the limit
                                int k = violIndices[y];

                                w[x, y] = this.smoothMetricInverse[j, k];
                            }
                        }

                        // Compute the KKT multipliers as: LHS = W^-1 * RHS. Note: W^-1 is always invertible as it is a sub-matrix of a positive definite matrix
                        DoubleArray lhs = w.Inv() * rhs;

                        // Now add the updates to the gradient
                        // Violating low limits: grad_j = grad_j + LHS_j 
                        // Violating high limits: grad_j = grad_j + RHS_j
                        for (int x = 0; x < violIndices.Count; x++)
                        {
                            // Get the index of the point violating the limit
                            int j = violIndices[x];

                            // Update the gradient based on the joint update
                            this.jointGradientInternal[i, j] += lhs[x];
                        }
                    }
                });
            
            // Do CHOMP update - Even if "doUpdate" is false, we do this as we want to check for joint limit violations
            this.jointTrajectoryInternal = this.jointTrajectoryInternal 
                - this.param.StepSize * this.jointGradientInternal * this.smoothMetricInverse;

            // Update original trajectory - set all internal points to new value
            for (int i = 0; i < this.numDOF; i++)
            {
                for (int j = 0; j < this.numInternal; j++)
                {
                    this.JointTrajectory[i, j + 1] = this.jointTrajectoryInternal[i, j];
                }
            }
        }

        /// <summary>
        /// Rewind to the previous trajectory by removing the last CHOMP update
        /// </summary>
        private void RewindLastCHOMPUpdate()
        {
            // Remove the last CHOMP update - add the gradient update to cancel it out
            this.jointTrajectoryInternal = this.jointTrajectoryInternal
                + this.param.StepSize * this.jointGradientInternal * this.smoothMetricInverse;

            // Update original trajectory - set all internal points to previous values
            for (int i = 0; i < this.numDOF; i++)
            {
                for (int j = 0; j < this.numInternal; j++)
                {
                    this.JointTrajectory[i, j + 1] = this.jointTrajectoryInternal[i, j];
                }
            }
        }

        /// <summary>
        /// Finds all the spheres in the robot's kinematic chain and stores them in "robotSpheres"
        /// Also gets the link on which each sphere lies and stores it in "linkIndexOfRobotSphere"
        /// </summary>
        /// <param name="kc"> Kinematic chain representing the robot </param>
        private void GetSpheresInKinematicChain(KinematicChain kc)
        {
            this.robotSpheres = new List<SphereElement>();
            this.linkIndexOfRobotSphere = new List<int>();

            // Set the robot at home position and get the list of spheres on the robot model
            DoubleArray home = DoubleArray.From(new double[] { 0, 0, 0, 0, 0, 0, 0 }).Transpose();

            // Transform the robot model based on the current joint configuration
            List<GeometryElementGroup3D> robotCollisionModel = kc.GetTransformedCollisionModels(home);

            // Iterate over all the links
            for (int j = 0; j < robotCollisionModel.Count; j++)
            {
                // some links may not have a collision model associated
                // a link's collision model may contain multiple geometry elements
                if (robotCollisionModel[j] == null)
                {
                    continue;
                }

                // Iterate over all elements for that particular link
                for (int k = 0; k < robotCollisionModel[j].ElementGroup.Count; k++)
                {
                    // Get the corresponding sphere 
                    GeometryElement3D a = robotCollisionModel[j].ElementGroup[k];
                    if (a is SphereElement)
                    {
                        // Add sphere to the list of spheres
                        this.robotSpheres.Add((SphereElement)a);

                        // Add the corresponding link index for that sphere
                        this.linkIndexOfRobotSphere.Add(j); 
                    }
                }
            }
        }

        /// <summary>
        /// At the start of each iteration, takes the current trajectory and computes
        /// the 3D position, velocity, acceleration and Jacobian for each sphere on the robot body
        /// </summary>
        private void DoPreComputationsForCHOMPIteration()
        {
            // Divide by dt and dt^2
            double oneByDt = 1.0 / this.dt;

            // Compute the jacobians AND sphere locations (in the world frame) for all the spheres on the robot in a parallel fashion
            Parallel.For(
                0,
                this.param.NumPoints,
                (i) =>
            {
                    // Get the joint configuration
                    DoubleArray point = this.JointTrajectory.GetSlice(0, this.numDOF - 1, i, i);

                    // Get the jacobian and the positions of all spheres in the world frame at that specific configuration
                    this.robotKinematicChain.GetSphereLocationAndPositionJacobianInWorldFrame(point, this.sphereJacobians[i], this.spherePositions[i]);
            });

            // Compute the velocities for each sphere on the robot
            // 1st order Backward differencing for Velocity -> vel(t) = (pos(t) - pos(t-1))/dt

            // FIRST compute the velocities all points other than START
            Parallel.For(
                1,
                this.param.NumPoints,
                (i) =>
                {
                    for (int j = 0; j < this.robotSpheres.Count; j++)
                    {
                        this.sphereVelocities[i][j][0] = (this.spherePositions[i][j][0] - this.spherePositions[i - 1][j][0]) * oneByDt;
                        this.sphereVelocities[i][j][1] = (this.spherePositions[i][j][1] - this.spherePositions[i - 1][j][1]) * oneByDt;
                        this.sphereVelocities[i][j][2] = (this.spherePositions[i][j][2] - this.spherePositions[i - 1][j][2]) * oneByDt;
                    }
                });

            // NEXT set velocity for start to the velocity of the second point
            for (int j = 0; j < this.robotSpheres.Count; j++)
            {
                MatrixExtensions.CopyValues(this.sphereVelocities[1][j], this.sphereVelocities[0][j]); // this.sphereVelocities[0][j], this.sphereVelocities[1][j]
            }

            // Compute the accelerations for each sphere on the robot
            // 2nd order Central differencing for velocity -> acc(t) = (pos(t+1) - 2*pos(t) + pos(t-1))/dt^2 = (vel(t+1) - vel(t))/dt => backward differencing on velocity

            // FIRST compute the accelerations all internal points
            Parallel.For(
                1,
                this.param.NumPoints - 1,
                (i) =>
                {
                    for (int j = 0; j < this.robotSpheres.Count; j++)
                    {
                        this.sphereAccelerations[i][j][0] = (this.sphereVelocities[i + 1][j][0] - this.sphereVelocities[i][j][0]) * oneByDt;
                        this.sphereAccelerations[i][j][1] = (this.sphereVelocities[i + 1][j][1] - this.sphereVelocities[i][j][1]) * oneByDt;
                        this.sphereAccelerations[i][j][2] = (this.sphereVelocities[i + 1][j][2] - this.sphereVelocities[i][j][2]) * oneByDt;
                    }
                });

            // NEXT set acceleration for start to the acceleration of the second point
            // For goal, set it to acceleration of previous point before goal
            for (int j = 0; j < this.robotSpheres.Count; j++)
            {
                // Set start point's acceleration to be equal to that of the first internal point
                MatrixExtensions.CopyValues(this.sphereAccelerations[1][j], this.sphereAccelerations[0][j]); // this.sphereAccelerations[0][j] = this.sphereAccelerations[1][j]

                // Set goal point's acceleration to be equal to that of the last internal point
                MatrixExtensions.CopyValues(this.sphereAccelerations[this.param.NumPoints - 2][j], this.sphereAccelerations[this.param.NumPoints - 1][j]); // this.sphereAccelerations[this.param.NumPoints - 1][j] = this.sphereAccelerations[this.param.NumPoints - 2][j]
            }
        }

        /// <summary>
        /// Computes the Obstacle gradient for the robot in the given environment, based on the current trajectory
        /// Obstacle cost measures the line integral of the body point costs as they move across a scalar field. Refer to the CHOMP IJRR paper for further details.
        /// In this function, we compute the gradients for obstacle avoidance and self collision avoidance as well as flags indicating if a trajectory point is in collision with obstacles or in self-collision.
        /// </summary>
        private void ComputeObsCostAndGradient()
        {
            Debug.Assert(this.obsGradient.size0 == this.numDOF, "Number of rows in obsGradient is not equal to the number of DOF");
            Debug.Assert(this.obsGradient.size1 == this.numInternal, "Number of rows in obsGradient is not equal to the number of DOF");
            Debug.Assert(this.obsCost.Count == this.numInternal, "Number of elements in obsCost is not equal to the number of internal points in the trajectory");

            // Initialize gradient and cost to zero
            this.obsGradient.FillValue(0.0);
            this.obsCost.FillValue(0.0);

            // Buffer for self collision cost and gradient computation
            double selfCollisionBuffer = 2 * this.param.SelfCollisionTolerance;

            // Run FK to get the 3D position of each sphere on the robot
            Parallel.For(
                0,
                this.numInternal,
                (i) =>
                {
                    // Initialize temporary variables
                    double[] xpos = new double[3];
                    double[] xvel = new double[3];
                    double[] xacc = new double[3];
                    double[] xvelUnit = new double[3];
                    double[] costGrad = new double[3];
                    double[] curv = new double[3];
                    double[] costGradProj = new double[3];
                    DoubleArray workspaceGrad = new DoubleArray(1, 3);

                    // Get the point index on the full trajectory (internal point "1" is point "2" on the trajectory)
                    int trajid = i + 1;

                    // Set collision flags for false initially
                    this.IsInternalPointInCollisionWithObstacles[i] = false;
                    this.IsInternalPointInCollisionWithSelf[i] = false;

                    // Iterate over all spheres
                    for (int j = 0; j < this.robotSpheres.Count; j++)
                    {
                        // Get position, velocity and accelerations - these have same size as the trajectory
                        MatrixExtensions.CopyValues(this.spherePositions[trajid][j], xpos); // xpos = this.spherePositions[trajid][j]
                        MatrixExtensions.CopyValues(this.sphereVelocities[trajid][j], xvel); // xvel = this.sphereVelocities[trajid][j]
                        MatrixExtensions.CopyValues(this.sphereAccelerations[trajid][j], xacc); // xacc = this.sphereAccelerations[trajid][j]

                        // Norm of Velocity
                        double xvelnorm = System.Math.Sqrt(xvel[0] * xvel[0] + xvel[1] * xvel[1] + xvel[2] * xvel[2]);
                        xvelnorm = Math.Max(xvelnorm, this.minVectorNorm); // bound the norm for numerical stability

                        // Compute 1/xvelnorm once for efficiency reasons
                        double oneByXvelnorm = 1.0 / xvelnorm;

                        // Normalize velocity vector
                        MatrixExtensions.CopyValues(xvel, xvelUnit); // xvelunit = xvel
                        MatrixExtensions.Scale(xvelUnit, oneByXvelnorm); // xvelUnit = xvel / xvelnorm

                        // Get the cost and gradient away from the nearest obstacle for that particular sphere
                        double cost = 0.0;
                        bool isSphereCollidingWithObs = this.obsDelegate(xpos, this.robotSpheres[j].Radius, this.param.EnvCollisionTolerance, out cost, out costGrad);

                        // In case we have a positive cost, we proceed further. 
                        // Else, this sphere is not close to any obstacles. We can proceed to the self collision check
                        if (cost > 0.0)
                        {
                            // Scale by a factor for the obstacle cost
                            cost *= this.param.EnvCollisionFactor;
                            MatrixExtensions.Scale(costGrad, this.param.EnvCollisionFactor); // costGrad = costGrad * this.param.EnvCollisionFactor

                            // WORKSPACE GRADIENT IS MADE OF TWO TERMS:
                            // 1) PROJECTION OF OBSTACLE GRADIENT IN DIRECTION ORTHOGONAL TO VELOCITY
                            // 2) CURVATURE VECTOR - PROJECTION OF ACCELERATION IN DIRECTION ORTHOGONAL TO VELOCITY
                            // G = (||x'|| * (I - xU' xU'^T) * costGrad) - (cost/||x'||)*(I - xU' xU'^T) * x''
                            // x' - velocity, x'' - acceleration, ||x'|| - 2-norm of velocity, I - 3 x 3 identity matrix, xU' - Unit vector pointing in direction of x' = x'/||x'||

                            // COMPUTE CURVATURE VECTOR
                            // Compute the inner product between the acceleration and velocity direction
                            double velAccIP = MatrixExtensions.VectorInnerProduct(xvelUnit, xacc); // velAccIP = xvelUnit^T * xacc

                            // Compute the COST scaled curvature vector (cost/||x'||) * k = cost * (1/||x'||) * ( I  - (x'x'^T/(||x'||^2))) x'' = cost * (1/||xvelNorm||) * [xacc - (xvelUnit * (xvelUnit^T * xacc))]
                            // This is a projection of the acceleration vector in the direction orthogonal to the velocity of the sphere.
                            // We do the scaling by the cost later
                            curv[0] = (xacc[0] - (xvelUnit[0] * velAccIP)) * oneByXvelnorm;
                            curv[1] = (xacc[1] - (xvelUnit[1] * velAccIP)) * oneByXvelnorm;
                            curv[2] = (xacc[2] - (xvelUnit[2] * velAccIP)) * oneByXvelnorm;

                            // COMPUTE OBSTACLE GRADIENT PROJECTION ON THE DIRECTION ORTHOGONAL TO VELOCITY
                            // Compute the inner product of the workspace gradient and the velocity directions 
                            double velCostGradIP = MatrixExtensions.VectorInnerProduct(xvelUnit, costGrad); // velCostGradIP = xvelUnit^T * costGrad

                            // Project the workspace gradient on the direction orthogonal to the sphere's velocity vector
                            costGradProj[0] = (costGrad[0] - (xvelUnit[0] * velCostGradIP)) * xvelnorm;
                            costGradProj[1] = (costGrad[1] - (xvelUnit[1] * velCostGradIP)) * xvelnorm;
                            costGradProj[2] = (costGrad[2] - (xvelUnit[2] * velCostGradIP)) * xvelnorm;

                            // Combine all these to get gradient in workspace = COSTGRADPROJ - cost * CURV
                            workspaceGrad[0, 0] = costGradProj[0] - cost * curv[0];
                            workspaceGrad[0, 1] = costGradProj[1] - cost * curv[1];
                            workspaceGrad[0, 2] = costGradProj[2] - cost * curv[2];
                        }
                        else
                        {
                            workspaceGrad[0, 0] = 0;
                            workspaceGrad[0, 1] = 0;
                            workspaceGrad[0, 2] = 0;
                        }

                        // If we have any sphere in collision with any of the obstacles (except the fixed base sphere), the traj point is in collision with obstacles
                        if ((isSphereCollidingWithObs) && (j > 0))
                        {
                            this.IsInternalPointInCollisionWithObstacles[i] = true;
                        }

                        // ----- NOW, WE CAN CONSIDER SELF COLLISIONS WITH OTHER LINKS ON THE ROBOT ----- //
                        // SELF COLLISION can be handled in the same way as any other obstacle. We use the CHOMP cost function (refer to CHOMP IJRR paper)
                        // which penalizes a sphere based on the euclidean distance to other spheres. 
                        // if SDist < 0
                        //    Cost = -SDist + 0.5*selfColBuffer
                        // else if 0 < SDist <= selfColBuffer
                        //    Cost = (0.5/selfColBuffer) * (SDist - selfColBuffer)^2
                        // else
                        //    Cost = 0
                        // where SDist is signed distance between surface of the spheres and selfColBuffer is a threshold for the gradient and cost computation
                        // Here, selfColBuffer = 2 * eps_Self
                        // We use the same CHOMP obstacle gradient for computing the update based on the self collision cost and add it to the previously computed gradient
                        // For each pair of spheres A & B, we compute the gradient of the cost in workspace that pushes both the spheres away from each other
                        // WorkspaceGradient = g_w. 
                        double cost_self = 0.0;
                        bool isSphereCollidingWithOtherRobotSpheres = false;
                        for (int x = 0; x < this.robotSpheres.Count; x++)
                        {
                            // Only check with respect to spheres that are atleast one link away from us.
                            if ((Math.Abs(this.linkIndexOfRobotSphere[j] - this.linkIndexOfRobotSphere[x]) <= 1) || (Math.Abs(j - x) <= 2))
                            {
                                continue;
                            }

                            // Get the vector between the sphere centers and store it in costGrad
                            MatrixExtensions.Subtract(xpos, this.spherePositions[trajid][x], costGrad); // costGrad = xpos - this.spherePositions[trajid][x]

                            // Get the distance between the centers
                            double centreDist = System.Math.Sqrt(costGrad[0] * costGrad[0] + costGrad[1] * costGrad[1] + costGrad[2] * costGrad[2]);

                            // Get the distance between the sphere surfaces
                            double surfDist = centreDist - this.robotSpheres[j].Radius - this.robotSpheres[x].Radius;

                            // Compute the cost and gradient for that sphere pair
                            double selfcost = 0.0;
                            if (surfDist > selfCollisionBuffer)
                            {
                                // If we are farther away than the tolerance, do not proceed further
                                continue;
                            }
                            else if (surfDist <= 0.0)
                            {
                                // Cost is distance from the surface (-ve of signed distance here) + a constant
                                // Gradient is the gradient of the actual distance (-surfDist) which is the unit vector pointing away from the obstacle
                                selfcost = -surfDist + 0.5 * selfCollisionBuffer;
                                double scal = -1.0 / centreDist;
                                MatrixExtensions.Scale(costGrad, scal); // costGrad = costGrad * scal
                                isSphereCollidingWithOtherRobotSpheres = true; // we have self collision with another sphere
                            }
                            else if ((surfDist > 0.0) && (surfDist <= selfCollisionBuffer))
                            {
                                // Gradient is again a scaled version of the unit vector pointing away from the obstacle
                                selfcost = (0.5 / selfCollisionBuffer) * (surfDist - selfCollisionBuffer) * (surfDist - selfCollisionBuffer);
                                double scal = ((surfDist / selfCollisionBuffer) - 1.0) * (1.0 / centreDist);
                                MatrixExtensions.Scale(costGrad, scal); // costGrad = costGrad * scal

                                // If we are less than epsilon distance away from another sphere, we set flag to true
                                if (surfDist <= this.param.SelfCollisionTolerance)
                                {
                                    isSphereCollidingWithOtherRobotSpheres = true;
                                }
                            }

                            // Scale by self collision factor
                            selfcost *= this.param.SelfCollisionFactor;
                            MatrixExtensions.Scale(costGrad, this.param.SelfCollisionFactor); // costGrad = costGrad * this.param.SelfCollisionFactor

                            // COMPUTE SELF COLLISION OBSTACLE GRADIENT PROJECTION ON THE DIRECTION ORTHOGONAL TO VELOCITY
                            // Compute the inner product of the workspace gradient and the velocity directions 
                            double velCostGradIP = MatrixExtensions.VectorInnerProduct(xvelUnit, costGrad);

                            // Project the workspace gradient on the direction orthogonal to the sphere's velocity vector
                            costGradProj[0] = (costGrad[0] - (xvelUnit[0] * velCostGradIP)) * xvelnorm;
                            costGradProj[1] = (costGrad[1] - (xvelUnit[1] * velCostGradIP)) * xvelnorm;
                            costGradProj[2] = (costGrad[2] - (xvelUnit[2] * velCostGradIP)) * xvelnorm;

                            // Combine all these to get gradient in workspace = COSTGRADPROJ - CURV
                            workspaceGrad[0, 0] += costGradProj[0] - selfcost * curv[0];
                            workspaceGrad[0, 1] += costGradProj[1] - selfcost * curv[1];
                            workspaceGrad[0, 2] += costGradProj[2] - selfcost * curv[2];

                            // Convert to config space via the transpose of the jacobian for that sphere
                            // Note: we use only the position jacobian
                            // WE HAVE TO ADD THIS GRADIENT FOR THE CURRENT SPHERE AND THE SPHERE WITH WHICH WE ARE COLLIDING AS WE NEED TO MOVE BOTH AWAY FROM EACH OTHER
                            // For sphere A, this gradient will be (Jac_A * workspaceGrad) 
                            // For sphere B, this gradient will be (Jac_B * -workspaceGrad) - we have a minus as we need to move sphere B in the opposite direction to get away from A
                            // So, sum is (Jac_A - Jac_B) * workspaceGrad
                            // Add this with the obstacle gradient and self collision gradients of all spheres
                            // configspaceGrad += (this.sphereJacobians[trajid][j].GetSlice(0, 2, 0, this.numDOF - 1)).T
                            //    * workspaceGrad;

                            // Add up cost across spheres
                            cost_self += selfcost;
                        }

                        // If we have any sphere in collision with any other spheres of the robot, the trajectory point is in self collision
                        if (isSphereCollidingWithOtherRobotSpheres)
                        {
                            this.IsInternalPointInCollisionWithSelf[i] = true;
                        }

                        // Finally convert to config space via the transpose of the jacobian for that sphere
                        // Note: we use only the position jacobian
                        // THIS IS THE GRADIENT DUE TO ALL OBSTACLES FOR THAT SPHERE ((1 x 3) * (3 x NDOF) = (1 x NDOF) vector)
                        DoubleArray configspaceGrad = workspaceGrad * this.sphereJacobians[trajid][j];

                        // Finally sum up gradients from obstacles and self-collision and add it to the overall obstacle gradient for that traj point
                        for (int y = 0; y < this.numDOF; y++)
                        {
                            this.obsGradient[y, i] += configspaceGrad[y];
                        }

                        // Add up cost from obstacles and self-collision for that traj point
                        // obstacle cost = sum_over_spheres(cost(sphere) * vel_norm_sphere)
                        // These is the size of the  internal trajectory
                        this.obsCost[i] += (cost + cost_self) * xvelnorm;
                    }
                });
        }
    }
}
